% desired command sequence
% goToWaypoint [40.437042, -79.948652, 0.5]
% goToWaypoint [40.436650, -79.948925, 0.5]
% goToWaypoint [40.436819, -79.948472, 0.5]
% goToWaypoint [40.437042, -79.948652, 0.5]
% halt

boat = 2;

sendCommand(boat, 'goToHeading', 200)
pause(3);
stat = getStatus(boat)
while (stat.started == 1)
    pause(0.1);
    stat = getStatus(boat)
end

disp('Done with the first part.');
pause;
sendCommand(boat, 'goToHeading', 0)
pause(3);
stat = getStatus(boat)
while (stat.started == 1)
    pause(0.1);
    stat = getStatus(boat)
end

disp('Done!');
% 
% sendCommand(boat, 'goToHeading', 0);
% pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed
% 
% stat = getStatus(boat);
% while (stat.started == 1)
%     pause(1);
%     stat = getStatus(boat);
% end
% 
% sendCommand(boat, 'goToWaypoint', 40.436819, -79.948472, 0.5);
% pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed
% 
% stat = getStatus(boat);
% while (stat.started == 1)
%     pause(1);
%     stat = getStatus(boat);
% end
% 
% sendCommand(boat, 'goToWaypoint', 40.437042, -79.948652, 0.5);
% pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed
% 
% stat = getStatus(boat);
% while (stat.started == 1)
%     pause(1);
%     stat = getStatus(boat);
% end
% 
% sendCommand(boat, 'halt');
